#include "../include/LocalMap.hpp"
namespace ORBSLAM
{
    LocalMap::LocalMap(KeyFrameDatabase* p_keyframe_database)
    {
        mp_keyframe_database = p_keyframe_database;
    }

    LocalMap::~LocalMap()
    {
    }

    void LocalMap::Set_param(Param *p_param)
    {
        mp_param = p_param;
        mp_matcher = new Matcher();
        mp_matcher->Set_matcher_parameters(50,100, 0.6, true, 60);
    }

    void LocalMap::Set_loopclosing(LoopClosing* p_loopclosing){
        mp_loopclosing = p_loopclosing;
    }

    void LocalMap::Set_map(Map *p_map)
    {
        mp_cur_map = p_map;
    }

    void LocalMap::Insert_keyframe(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_new_keyframe);
        mlp_new_keyframe.push_back(p_keyframe);
        mb_abort_ba = true;
    }

    bool LocalMap::Set_not_stop(bool flag)
    {
        unique_lock<mutex> lock(mmutex_stop);
        if (flag && mb_stoped)
        {
            return false;
        }
        mb_not_stop = flag;
        return true;
    }

    void LocalMap::Interrupt_ba()
    {
        mb_abort_ba = true;
    }

    int LocalMap::Keyframe_in_queue()
    {
        unique_lock<mutex> lock(mmutex_new_keyframe);
        return mlp_new_keyframe.size();
    }

    void LocalMap::Update_local_keyframe(Frame &cur_frame)
    {
        map<KeyFrame *, int> map_keyframe_counter;
        for (int i = 0; i < cur_frame.mn_feature_num; i++)
        {
            MapPoint *p_mpt = cur_frame.mvp_mappoints[i];
            if (p_mpt)
            {
                if (!p_mpt->Is_bad())
                {
                    const map<KeyFrame *, int> map_observation = p_mpt->Get_observation();
                    for (map<KeyFrame *, int>::const_iterator mit = map_observation.begin(); mit != map_observation.end(); mit++)
                    {
                        map_keyframe_counter[mit->first]++;
                    }
                }
            }
            else
            {
                cur_frame.mvp_mappoints[i] = NULL;
            }
        }

        int max = 0;
        KeyFrame *p_keyframe_maxobservation = static_cast<KeyFrame *>(NULL);
        mvp_local_keyframe.clear();
        mvp_local_keyframe.reserve(3 * map_keyframe_counter.size());

        vector<KeyFrame *> vp_tmp_local_keyframe;
        vp_tmp_local_keyframe.reserve(3 * map_keyframe_counter.size());

        int tmp_keyframe_id = 0;
        for (map<KeyFrame *, int>::const_iterator mit = map_keyframe_counter.begin(); mit != map_keyframe_counter.end(); mit++)
        {
            KeyFrame *p_keyframe = mit->first;
            if (p_keyframe->Is_bad())
            {
                continue;
            }
            if (mit->second > max)
            {
                max = mit->second;
                p_keyframe_maxobservation = p_keyframe;
                tmp_keyframe_id = p_keyframe->mn_keyframe_id;
            }
            else if (mit->second == max)
            {
                if (p_keyframe->mn_keyframe_id > tmp_keyframe_id)
                {
                    max = mit->second;
                    p_keyframe_maxobservation = p_keyframe;
                    tmp_keyframe_id = p_keyframe->mn_keyframe_id;
                }
            }

            mvp_local_keyframe.push_back(p_keyframe);
            p_keyframe->mn_localkeyframe_tmpid = cur_frame.mn_frame_id;
        }

        for (vector<KeyFrame *>::const_iterator vit = mvp_local_keyframe.begin(); vit != mvp_local_keyframe.end(); vit++)
        {
            if (mvp_local_keyframe.size() > 80)
            {
                break;
            }
            KeyFrame *p_keyframe = *vit;
            const vector<KeyFrame *> vp_friend_keyframe = p_keyframe->Get_best_covisibility_keyframe(10);
            for (vector<KeyFrame *>::const_iterator vit2 = vp_friend_keyframe.begin(); vit2 != vp_friend_keyframe.end(); vit2++)
            {
                KeyFrame *p_friend_Keyframe = *vit2;
                if (!p_friend_Keyframe->Is_bad())
                {
                    if (p_friend_Keyframe->mn_localkeyframe_tmpid != cur_frame.mn_frame_id)
                    {
                        vp_tmp_local_keyframe.push_back(p_friend_Keyframe);
                        p_friend_Keyframe->mn_localkeyframe_tmpid = cur_frame.mn_frame_id;
                        //? find one then break?
                        // break;
                    }
                }
            }
            const set<KeyFrame *> sp_child = p_keyframe->Get_child();
            for (set<KeyFrame *>::const_iterator sit = sp_child.begin(); sit != sp_child.end(); sit++)
            {
                KeyFrame *p_child_keyframe = *sit;
                if (!p_child_keyframe->Is_bad())
                {
                    if (p_child_keyframe->mn_localkeyframe_tmpid != cur_frame.mn_frame_id)
                    {
                        vp_tmp_local_keyframe.push_back(p_child_keyframe);
                        p_child_keyframe->mn_localkeyframe_tmpid = cur_frame.mn_frame_id;
                        //? find one then break?
                        // break;
                    }
                }
            }

            KeyFrame *p_parent_keyframe = p_keyframe->Get_parent();
            if (p_parent_keyframe)
            {
                if (p_parent_keyframe->mn_localkeyframe_tmpid != cur_frame.mn_frame_id)
                {
                    vp_tmp_local_keyframe.push_back(p_parent_keyframe);
                    p_parent_keyframe->mn_localkeyframe_tmpid = cur_frame.mn_frame_id;
                }
            }
        }

        for (int i = 0; i < vp_tmp_local_keyframe.size(); i++)
        {
            mvp_local_keyframe.push_back(vp_tmp_local_keyframe[i]);
        }
        if (p_keyframe_maxobservation)
        {
            p_keyframe_maxobservation->Set_as_ref_keyframe(true);
            cur_frame.mp_ref_keyframe = p_keyframe_maxobservation;
        }
    }

    void LocalMap::Update_local_mappoint(Frame &cur_frame)
    {
        mvp_local_mappoint.clear();
        int count_pts = 0;
        for (vector<KeyFrame *>::const_iterator vit_keyframe = mvp_local_keyframe.begin(); vit_keyframe != mvp_local_keyframe.end(); vit_keyframe++)
        {
            KeyFrame *p_keyframe = *vit_keyframe;
            const vector<MapPoint *> vp_mpt = p_keyframe->Get_vect_mappoints();

            for (vector<MapPoint *>::const_iterator vit_mpt = vp_mpt.begin(); vit_mpt != vp_mpt.end(); vit_mpt++)
            {
                MapPoint *p_mpt = *vit_mpt;
                if (!p_mpt)
                {
                    continue;
                }
                if (p_mpt->mn_localmappoint_tmpid == cur_frame.mn_frame_id)
                {
                    continue;
                }
                if (!p_mpt->Is_bad())
                {
                    count_pts++;
                    mvp_local_mappoint.push_back(p_mpt);
                    p_mpt->mn_localmappoint_tmpid = cur_frame.mn_frame_id;
                }
            }
        }
    }

    void LocalMap::Set_accept_keyframe(bool flag)
    {
        unique_lock<mutex> lock(mmutex_accept);
        mb_accept_keyframe = flag;
    }

    bool LocalMap::Accept_keyframes()
    {
        unique_lock<mutex> lock(mmutex_accept);
        return mb_accept_keyframe;
    }

    bool LocalMap::Check_if_new_keyframe_exist()
    {
        unique_lock<mutex> lock(mmutex_new_keyframe);
        return (!mlp_new_keyframe.empty());
    }

    void LocalMap::Process_new_keyframe()
    {
        {
            unique_lock<mutex> lock(mmutex_new_keyframe);
            mp_current_keyframe = mlp_new_keyframe.front();
            mlp_new_keyframe.pop_front();
        }

        const vector<MapPoint *> vp_mappoints = mp_current_keyframe->Get_vect_mappoints();
        for (int i = 0; i < vp_mappoints.size(); i++)
        {
            MapPoint *p_mpt = vp_mappoints[i];
            if (p_mpt)
            {
                if (!p_mpt->Is_bad())
                {
                    if (!p_mpt->Is_in_keyframe(mp_current_keyframe))
                    {
                        p_mpt->Add_observation(mp_current_keyframe, i);
                        p_mpt->Update_normal_and_depth();
                        p_mpt->Compute_distinctive_descriptors();
                    }
                    else
                    {
                        // TODO
                        //  mlp_recently_added_mappoints.push_back(p_mpt);
                    }
                }
            }
        }
        mp_current_keyframe->Update_connections();
        // mp_cur_map->Add_key_frame(mp_current_keyframe);
    }

    void LocalMap::Mappoint_culling()
    {
        list<MapPoint *>::iterator lit = mlp_recently_added_mappoints.begin();
        const unsigned long int ncurrent_keyframe_id = mp_current_keyframe->mn_keyframe_id;

        int nth_obs = 2;

        int borrar = mlp_recently_added_mappoints.size();

        while (lit != mlp_recently_added_mappoints.end())
        {
            MapPoint *p_mpt = *lit;
            if (p_mpt->Is_bad())
            {
                lit = mlp_recently_added_mappoints.erase(lit);
            }
            else if (p_mpt->Get_found_ratio() < 0.25f)
            {
                p_mpt->Set_bad_flag();
                lit = mlp_recently_added_mappoints.erase(lit);
            } // TODO
            else if (((int)ncurrent_keyframe_id - (int)p_mpt->mn_first_keyframe_id) >= 6 && p_mpt->Get_observation_num() <= nth_obs)
            {
                p_mpt->Set_bad_flag();
                lit = mlp_recently_added_mappoints.erase(lit);
            }
            else if (((int)ncurrent_keyframe_id - (int)p_mpt->mn_first_keyframe_id) >= 5)
            {
                lit = mlp_recently_added_mappoints.erase(lit);
            }
            else
            {
                ++lit;
                borrar--;
            }
        }
        cout << "erased mappoint num:" << borrar << endl;
    }

    void LocalMap::Create_new_mappoint()
    {
        int nn = 20;
        vector<KeyFrame *> vp_neighor_keyframe = mp_current_keyframe->Get_best_covisibility_keyframe(nn);

        // mp_matcher->Set_ratio(0.6);

        auto Rcw1 = mp_current_keyframe->Get_Rcw_();
        auto Rwc1 = mp_current_keyframe->Get_Rwc_();
        auto tcw1 = mp_current_keyframe->Get_tcw_();

        auto twc1 = mp_current_keyframe->Get_twc_();

        auto Tcw1 = mp_current_keyframe->Get_Tcw_();

        const float ratio_factor = 1.5 * mp_current_keyframe->md_scale_factor;

        const float mediandepth1 = mp_current_keyframe->Compute_scene_median_depth(2);

        for (int i = 0; i < vp_neighor_keyframe.size(); i++)
        {

            int n_new_mappoints = 0;

            if (Check_if_new_keyframe_exist() && i > 0)
            {
                return;
            }
            KeyFrame *p_keyframe2 = vp_neighor_keyframe[i];

            // cout << "keyframe" << mp_current_keyframe->mn_KF_id << "(frame" << mp_current_keyframe->mn_frame_id << ") with keyframe" << p_keyframe2->mn_KF_id << "(frame" << p_keyframe2->mn_frame_id << ") create newpoints..." << endl;

            auto twc2 = p_keyframe2->Get_twc_();

            auto vbaseline = twc1 - twc2;

            const float baseline = norm(vbaseline);

            const float mediandepth2 = p_keyframe2->Compute_scene_median_depth(2);

            const float ratio_baseline_depth = 2 * baseline / (mediandepth2 + mediandepth1);

            if (ratio_baseline_depth < 0.01)
            {
                // cout << "baseline/depth too small, dont generate new mappoints." << endl;
                continue;
            }

            auto F12 = ComputeF12(mp_current_keyframe, p_keyframe2);

            vector<pair<int, int>> v_match_idx;

            mp_matcher->Search_for_triangulation(mp_current_keyframe, p_keyframe2, F12, v_match_idx, false, false);

            // Debuger::Show_two_image_match(mp_current_keyframe,p_keyframe2,v_match_idx);

            auto Rcw2 = p_keyframe2->Get_Rcw_();
            auto Rwc2 = p_keyframe2->Get_Rwc_();
            auto tcw2 = p_keyframe2->Get_tcw_();
            auto Tcw2 = p_keyframe2->Get_Tcw_();

            const int nmatches = v_match_idx.size();
            // cout << "search_for_triangulation nmatch:" << nmatches << endl;
            int n_bad_small_parallex = 0;
            int n_bad_negtive_z = 0;
            int n_bad_projection_err = 0;
            int n_bad_scale_consistance = 0;
            for (int ikp = 0; ikp < nmatches; ikp++)
            {
                const int &idx1 = v_match_idx[ikp].first;
                const int &idx2 = v_match_idx[ikp].second;
                const KeyPoint &kp1 = mp_current_keyframe->mv_orb_unkeypoints[idx1];
                const KeyPoint &kp2 = p_keyframe2->mv_orb_unkeypoints[idx2];

                auto xn1 = Converter::Uv_to_3dnormal_(kp1.pt);
                auto xn2 = Converter::Uv_to_3dnormal_(kp2.pt);

                auto ray1 = Rwc1 * xn1;
                auto ray2 = Rwc2 * xn2;

                const float cosparallax = ray1.dot(ray2) / (cv::norm(ray1) * cv::norm(ray2));

                cv::Matx31f x3d;
                bool b_estimate = false;

                if (cosparallax < 0.9998)
                {
                    Matx14f A_r0 = xn1(0) * Tcw1.row(2) - Tcw1.row(0);
                    Matx14f A_r1 = xn1(1) * Tcw1.row(2) - Tcw1.row(1);
                    Matx14f A_r2 = xn2(0) * Tcw2.row(2) - Tcw2.row(0);
                    Matx14f A_r3 = xn2(1) * Tcw2.row(2) - Tcw2.row(1);

                    Matx44f A{A_r0(0), A_r0(1), A_r0(2), A_r0(3), A_r1(0), A_r1(1), A_r1(2), A_r1(3), A_r2(0), A_r2(1), A_r2(2), A_r2(3), A_r3(0), A_r3(1), A_r3(2), A_r3(3)};

                    Matx44f u, vt;
                    Matx41f w;
                    SVD::compute(A, w, u, vt, SVD::MODIFY_A | SVD::FULL_UV);

                    Matx41f x_3d_h = vt.row(3).t();

                    if (x_3d_h(3) == 0)
                    {
                        continue;
                    }

                    x3d = Matx31f(x_3d_h.get_minor<3, 1>(0, 0)(0) / x_3d_h(3), x_3d_h.get_minor<3, 1>(0, 0)(1) / x_3d_h(3), x_3d_h.get_minor<3, 1>(0, 0)(2) / x_3d_h(3));

                    b_estimate = true;
                }
                else
                {
                    n_bad_small_parallex++;
                    continue;
                }

                Matx13f x3dt = x3d.t();

                if (!b_estimate)
                {
                    continue;
                }

                float z1 = Rcw1.row(2).dot(x3dt) + tcw1(2);
                if (z1 <= 0)
                {
                    n_bad_negtive_z++;
                    continue;
                }
                float z2 = Rcw2.row(2).dot(x3dt) + tcw2(2);
                if (z2 <= 0)
                {
                    n_bad_negtive_z++;
                    continue;
                }

                const float &sigmasquare1 = mp_current_keyframe->mvd_level_sigma2[kp1.octave];
                const float x1 = Rcw1.row(0).dot(x3dt) + tcw1(0);
                const float y1 = Rcw1.row(1).dot(x3dt) + tcw1(1);
                const float invz1 = 1.0 / z1;

                Point2f uv1 = Converter::P3d_to_pointt2f(Point3f(x1, y1, z1));
                float errx1 = uv1.x - kp1.pt.x;
                float erry1 = uv1.y - kp1.pt.y;

                if ((errx1 * errx1 + erry1 * erry1) > 5.991 * sigmasquare1)
                {
                    n_bad_projection_err++;
                    continue;
                }

                const float &sigmasquare2 = p_keyframe2->mvd_level_sigma2[kp2.octave];
                const float x2 = Rcw2.row(0).dot(x3dt) + tcw2(0);
                const float y2 = Rcw2.row(1).dot(x3dt) + tcw2(1);
                const float invz2 = 1.0 / z2;

                Point2f uv2 = Converter::P3d_to_pointt2f(Point3f(x2, y2, z2));
                float errx2 = uv2.x - kp2.pt.x;
                float erry2 = uv2.y - kp2.pt.y;
                if ((errx2 * errx2 + erry2 * erry2) > 5.991 * sigmasquare2)
                {
                    n_bad_projection_err++;
                    continue;
                }

                auto normal1 = x3d - twc1;
                float dist1 = norm(normal1);

                auto normal2 = x3d - twc2;
                float dist2 = norm(normal2);

                if (dist1 == 0 || dist2 == 0)
                {
                    continue;
                }

                const float ratio_dist = dist1 / dist2;
                const float ratio_octave = mp_current_keyframe->mvd_scale_factor[kp1.octave] / p_keyframe2->mvd_scale_factor[kp2.octave];

                if (ratio_dist * ratio_factor < ratio_octave || ratio_dist > ratio_octave * ratio_factor)
                {
                    n_bad_scale_consistance++;
                    continue;
                }

                Mat p3d(x3d);
                MapPoint *p_mpt = new MapPoint(p3d);
                p_mpt->Set_ref_keframe(mp_current_keyframe, p_keyframe2);
                p_mpt->Add_observation(mp_current_keyframe, idx1);
                p_mpt->Add_observation(p_keyframe2, idx2);
                // if (mp_current_keyframe->mn_KF_id < p_keyframe2->mn_KF_id)
                // {
                //     p_mpt->Add_observation(mp_current_keyframe, idx1);
                //     p_mpt->Add_observation(p_keyframe2, idx2);
                // }
                // else
                // {
                //     p_mpt->Add_observation(p_keyframe2, idx2);
                //     p_mpt->Add_observation(mp_current_keyframe, idx1);
                // }
                mp_current_keyframe->Add_mappoint(p_mpt, idx1);
                p_keyframe2->Add_mappoint(p_mpt, idx2);
                p_mpt->Compute_distinctive_descriptors();
                p_mpt->Update_normal_and_depth();
                mp_cur_map->Add_mappoint(p_mpt);
                p_mpt->Set_map(mp_cur_map);

                mp_current_keyframe->Update_connections();
                p_keyframe2->Update_connections();

                mlp_recently_added_mappoints.push_back(p_mpt);
                n_new_mappoints++;
            }

            // cout << "n_new_mappoints:" << n_new_mappoints << endl;
            // cout << "n_bad_small_parralex:" << n_bad_small_parallex << endl;
            // cout << "n_bad_negtive_z:" << n_bad_negtive_z << endl;
            // cout << "n_bad_projection_err:" << n_bad_projection_err << endl;
            // cout << "n_bad_scale_consistance:" << n_bad_scale_consistance << endl;
        }
    }

    void LocalMap::Request_stop(){
        unique_lock<mutex> lock(mmutex_stop);
        mb_stop_request = true;
        unique_lock<mutex> lock2(mmutex_new_keyframe);
        mb_abort_ba = true;
    }

    void LocalMap::Empty_queue(){
        while(Check_if_new_keyframe_exist()){
            Process_new_keyframe();
        }
    }

    void LocalMap::Keyframe_culling()
    {
        mp_current_keyframe->Update_best_covisibles();
        vector<KeyFrame *> vp_local_keyframe = mp_current_keyframe->Get_all_covisibility_keyframe();

        float redundant_th = 0.8;//TODO

        for (auto p_keyframe : vp_local_keyframe)
        {
            if (p_keyframe->mn_keyframe_id == p_keyframe->Get_map()->Get_init_KFid() || p_keyframe->Is_bad())
            {
                continue;
            }
            const vector<MapPoint *> vp_mpt = p_keyframe->Get_vect_mappoints();
            const int th_obs = 3;

            int n_redundant_obs = 0;

            int n_mpt = 0;
            int n_bad = 0;

            for (int i = 0; i < vp_mpt.size(); i++)
            {
                MapPoint *p_mpt = vp_mpt[i];
                if (p_mpt)
                {
                    if (!p_mpt->Is_bad())
                    {
                        n_mpt++;
                        if (p_mpt->Get_observation_num() > th_obs)
                        {
                            const int &scalelevel = p_keyframe->mv_orb_unkeypoints[i].octave;
                            const map<KeyFrame *, int> obs = p_mpt->Get_observation();
                            int n_obs = 0;
                            for (auto obsi : obs)
                            {
                                KeyFrame *p_keyframei = obsi.first;
                                if (p_keyframei == p_keyframe)
                                {
                                    continue;
                                }
                                int index = obsi.second;
                                int scaleleveli = -1;
                                scaleleveli = p_keyframei->mv_orb_unkeypoints[index].octave;

                                if (scaleleveli <= scalelevel + 1)
                                {
                                    n_obs++;
                                    if (n_obs > th_obs)
                                    {
                                        break;
                                    }
                                }
                            }
                            if (n_obs > th_obs)
                            {
                                n_redundant_obs++;
                            }
                        }
                    }
                    else
                    {
                        n_bad++;
                    }
                }
            }
            if (n_redundant_obs > redundant_th * n_mpt)
            {
                //TODO when to erase?
                if (!p_keyframe->Used_as_ref_keyframe())
                {
                    p_keyframe->Set_bad_flag();
                    mp_keyframe_database->Erase(p_keyframe);
                }
            }
        }
    }

    Matx33f LocalMap::ComputeF12(KeyFrame *&p_keyframe1, KeyFrame *&p_keyframe2)
    {
        auto R1w = p_keyframe1->Get_Rcw_();
        auto R2w = p_keyframe2->Get_Rcw_();
        auto t1w = p_keyframe1->Get_tcw_();
        auto t2w = p_keyframe2->Get_tcw_();

        auto R12 = R1w * R2w.t();
        auto t12 = -R1w * R2w.t() * t2w + t1w;

        auto t12x = Skew_symmetric_matrix(t12);

        const auto &k = mp_param->mm_camera_intrinsics_;

        return k.t().inv() * t12x * R12 * k.inv();
    }

    Matx33f LocalMap::Skew_symmetric_matrix(const Matx31f &v)
    {
        Matx33f skew{0.f, -v(2), v(1), v(2), 0.f, -v(0), -v(1), v(0), 0.f};
        return skew;
    }

    void LocalMap::Search_in_neighbors()
    {
        int nn = 20;

        const vector<KeyFrame *> vp_keyframe_neighbal1 = mp_current_keyframe->Get_best_covisibility_keyframe(nn);
        vector<KeyFrame *> vp_target_keyframe;
        vector<KeyFrame *> vp_target_keyframe_tmp;

        // TODO
        vp_target_keyframe.reserve(120);
        vp_target_keyframe_tmp.reserve(20);
        for (auto p_keyframe : vp_keyframe_neighbal1)
        {
            if (p_keyframe->Is_bad() || p_keyframe->mn_mark_fuse_id == mp_current_keyframe->mn_keyframe_id)
            {
                continue;
            }
            vp_target_keyframe_tmp.push_back(p_keyframe);
            vp_target_keyframe.push_back(p_keyframe);
            p_keyframe->mn_mark_fuse_id = mp_current_keyframe->mn_keyframe_id;
        }

        for (auto p_keyframe : vp_target_keyframe_tmp)
        {
            // TODO
            const vector<KeyFrame *> vp_keyframe_neighbal2 = p_keyframe->Get_best_covisibility_keyframe(5);
            for (auto p_keyframe2 : vp_keyframe_neighbal2)
            {
                if (p_keyframe2->Is_bad() || p_keyframe2->mn_mark_fuse_id == mp_current_keyframe->mn_keyframe_id || p_keyframe2->mn_keyframe_id == mp_current_keyframe->mn_keyframe_id)
                {
                    continue;
                }
                vp_target_keyframe.push_back(p_keyframe2);
                p_keyframe2->mn_mark_fuse_id = mp_current_keyframe->mn_keyframe_id;
            }

            if (mb_abort_ba)
            {
                break;
            }
        }
        // TODO
        // imu

        int n_fused = 0;
        int tmp_n_fused;
        vector<MapPoint *> vp_mappoint_match = mp_current_keyframe->Get_vect_mappoints();
        for (auto p_keyframe : vp_target_keyframe)
        {
            tmp_n_fused = mp_matcher->Fuse(p_keyframe, vp_mappoint_match);
            n_fused = n_fused + tmp_n_fused;
        }
        if (mb_abort_ba)
        {
            return;
        }

        vector<MapPoint *> vp_candidate_fuse_mpt;
        vp_candidate_fuse_mpt.reserve(vp_target_keyframe.size() * vp_mappoint_match.size());

        for (auto p_keyframe : vp_target_keyframe)
        {
            vector<MapPoint *> vp_mpt_keyframe = p_keyframe->Get_vect_mappoints();
            for (auto p_mpt : vp_mpt_keyframe)
            {
                if (!p_mpt)
                {
                    continue;
                }
                if (p_mpt->Is_bad() || p_mpt->mn_mark_fuse_id == mp_current_keyframe->mn_keyframe_id)
                {
                    continue;
                }
                p_mpt->mn_mark_fuse_id = mp_current_keyframe->mn_keyframe_id;
                vp_candidate_fuse_mpt.push_back(p_mpt);
            }
        }

        tmp_n_fused = mp_matcher->Fuse(mp_current_keyframe, vp_candidate_fuse_mpt);
        n_fused = n_fused + tmp_n_fused;

        // vp_mappoint_match = mp_current_keyframe->Get_vect_mappoints();

        // for (auto p_mpt : vp_mappoint_match)
        // {
        //     if (p_mpt)
        //     {
        //         if (!p_mpt->Is_bad())
        //         {
        //             p_mpt->Compute_distinctive_descriptors();
        //             p_mpt->Update_normal_and_depth();
        //         }
        //     }
        // }
        mp_current_keyframe->Update_connections();

        cout << "n_fused:" << n_fused << endl;
    }

    bool LocalMap::Stop()
    {
        unique_lock<mutex> lock(mmutex_stop);
        if (mb_stop_request && !mb_not_stop)
        {
            mb_stoped = true;
            cout << "local maping stop" << endl;
            return true;
        }
        return false;
    }

    bool LocalMap::Is_stoped()
    {
        unique_lock<mutex> lock(mmutex_stop);
        return mb_stoped;
    }

    bool LocalMap::Check_finish()
    {
        unique_lock<mutex> lock(mmutex_finish);
        return mb_finish_requested;
    }

    void LocalMap::Reset_if_requested()
    {
        bool executed_reset = false;
        {
            unique_lock<mutex> lock(mmutex_reset);
            if (mb_reset_requested)
            {
                executed_reset = true;
                cout << "LM: Reseting map in local mapping" << endl;

                mlp_new_keyframe.clear();
                mlp_recently_added_mappoints.clear();
                mb_reset_requested = false;

                // TODO imu relate

                cout << "LM: end reseting local mapping" << endl;
            }
        }
        if (executed_reset)
        {
            cout << "LM: reset free the mutex" << endl;
        }
    }

    void LocalMap::Release(){
        unique_lock<mutex> lock1(mmutex_stop);
        unique_lock<mutex> lock2(mmutex_finish);
        if(mb_finished){
            return;
        }
        mb_stoped = false;
        mb_stop_request = false;
        for(auto p_keyframei:mlp_new_keyframe){
            delete p_keyframei;
        }
        mlp_new_keyframe.clear();
        
    }

    void LocalMap::Set_finish()
    {
        unique_lock<mutex> lock1(mmutex_finish);
        mb_finished = true;
        unique_lock<mutex> lock2(mmutex_stop);
        mb_stoped = true;
    }

    void LocalMap::Run()
    {
        while (1)
        {
            Set_accept_keyframe(false);
            clock_t start_time, end_time;

            bool b_newkeyframe = Check_if_new_keyframe_exist();
            if (b_newkeyframe)
            {
                // start_time = clock();
                Process_new_keyframe();
                // end_time = clock();
                // cout << "run process_new_keyframe time: " << (double)(end_time - start_time) / CLOCKS_PER_SEC << "s" << endl;

                // int n_before_mappoint_culling = 0;
                // for (auto p_mpt : mp_current_keyframe->Get_vect_mappoints())
                // {
                //     if (!p_mpt)
                //     {
                //         continue;
                //     }
                //     if (p_mpt->Is_bad())
                //     {
                //         continue;
                //     }
                //     n_before_mappoint_culling++;
                // }
                // cout << "n_before_mappoint_culling:" << n_before_mappoint_culling << endl;

                // cout << "before mappoint culling, valuable pts num:" << endl;
                // for (auto p_keyframe : mp_current_keyframe->Get_all_covisibility_keyframe())
                // {
                //     int n_pts = 0;
                //     for (auto p_mpt : p_keyframe->Get_vect_mappoints())
                //     {
                //         if (!p_mpt)
                //         {
                //             continue;
                //         }
                //         if (p_mpt->Is_bad())
                //         {
                //             continue;
                //         }
                //         n_pts++;
                //     }
                //     cout << "keyframe" << p_keyframe->mn_KF_id << ":" << n_pts << endl;
                // }

                // start_time = clock();
                Mappoint_culling();
                // end_time = clock();
                // cout << "run mappoint_culling time: " << (double)(end_time - start_time) / CLOCKS_PER_SEC << "s" << endl;

                // cout << "before creation, valuable pts num:" << endl;

                // int n_before_creation = 0;
                // for (auto p_mpt : mp_current_keyframe->Get_vect_mappoints())
                // {
                //     if (!p_mpt)
                //     {
                //         continue;
                //     }
                //     if (p_mpt->Is_bad())
                //     {
                //         continue;
                //     }
                //     n_before_creation++;
                // }
                // cout << "keyframe" << mp_current_keyframe->mn_KF_id << ":" << n_before_creation << endl;

                // for (auto p_keyframe : mp_current_keyframe->Get_all_covisibility_keyframe())
                // {
                //     int n_pts = 0;
                //     for (auto p_mpt : p_keyframe->Get_vect_mappoints())
                //     {
                //         if (!p_mpt)
                //         {
                //             continue;
                //         }
                //         if (p_mpt->Is_bad())
                //         {
                //             continue;
                //         }
                //         n_pts++;
                //     }
                //     cout << "keyframe" << p_keyframe->mn_KF_id << ":" << n_pts << endl;
                // }

                start_time = clock();
                Create_new_mappoint();
                end_time = clock();
                cout << "run create_new_mappoint time: " << (double)(end_time - start_time) / CLOCKS_PER_SEC << "s" << endl;

                // cout << "after creation, valuable pts num:" << endl;

                // int n_after_creation = 0;
                // for (auto p_mpt : mp_current_keyframe->Get_vect_mappoints())
                // {
                //     if (!p_mpt)
                //     {
                //         continue;
                //     }
                //     if (p_mpt->Is_bad())
                //     {
                //         continue;
                //     }
                //     n_after_creation++;
                // }
                // cout << "keyframe" << mp_current_keyframe->mn_KF_id << ":" << n_after_creation << endl;
                // for (auto p_keyframe : mp_current_keyframe->Get_all_covisibility_keyframe())
                // {
                //     int n_pts = 0;
                //     for (auto p_mpt : p_keyframe->Get_vect_mappoints())
                //     {
                //         if (!p_mpt)
                //         {
                //             continue;
                //         }
                //         if (p_mpt->Is_bad())
                //         {
                //             continue;
                //         }
                //         n_pts++;
                //     }
                //     cout << "keyframe" << p_keyframe->mn_KF_id << ":" << n_pts << endl;
                // }

                mb_abort_ba = false;

                if (!Check_if_new_keyframe_exist())
                {
                    start_time = clock();
                    Search_in_neighbors();
                    end_time = clock();
                    cout << "run search_in_neighbors time: " << (double)(end_time - start_time) / CLOCKS_PER_SEC << "s" << endl;

                    // cout << "after fusion, valuable pts num:" << endl;

                    // int n_after_fusion = 0;
                    // for (auto p_mpt : mp_current_keyframe->Get_vect_mappoints())
                    // {
                    //     if (!p_mpt)
                    //     {
                    //         continue;
                    //     }
                    //     if (p_mpt->Is_bad())
                    //     {
                    //         continue;
                    //     }
                    //     n_after_fusion++;
                    // }
                    // cout << "keyframe" << mp_current_keyframe->mn_KF_id << ":" << n_after_fusion << endl;
                    // for (auto p_keyframe : mp_current_keyframe->Get_all_covisibility_keyframe())
                    // {
                    //     int n_pts = 0;
                    //     for (auto p_mpt : p_keyframe->Get_vect_mappoints())
                    //     {
                    //         if (!p_mpt)
                    //         {
                    //             continue;
                    //         }
                    //         if (p_mpt->Is_bad())
                    //         {
                    //             continue;
                    //         }
                    //         n_pts++;
                    //     }
                    //     cout << "keyframe" << p_keyframe->mn_KF_id << ":" << n_pts << endl;
                    // }
                }

                bool b_done_ba = false;
                if (!Check_if_new_keyframe_exist())
                {
                    if (mp_cur_map->Keyframes_in_map() > 2)
                    {
                        cout << "begin local ba..." << endl;
                        Optimizer::Local_bundadjustment(mp_current_keyframe, &mb_abort_ba, mp_current_keyframe->Get_map());
                        cout << "local ba end." << endl;
                        b_done_ba = true;
                    }

                    if (b_done_ba)
                    {
                    }

                    Keyframe_culling();
                }

                mp_loopclosing->Insert_keyframe(mp_current_keyframe);

                // TODO insert keyframe to loopclosing
            }
            else if (Stop())
            {
                while (Is_stoped() && !Check_finish())
                {
                    usleep(3000);
                }
                if (Check_finish())
                {
                    break;
                }
            }

            Reset_if_requested();

            Set_accept_keyframe(true);

            if (Check_finish())
            {
                break;
            }

            usleep(3000);

            // cout << "in localmapping..." << endl;
            // sleep(1);
        }

        Set_finish();
    }
}